import math
import csv
from typing import List, Tuple, Dict, Optional

import matplotlib.pyplot as plt
from shapely.geometry import Polygon, LineString, GeometryCollection
from shapely.ops import unary_union
from pyproj import Transformer


def plan_coverage_path(
    lat_list: List[float],
    lon_list: List[float],
    fov_deg: float = 75.0,
    altitude_m: float = 8.0,
    overlap_ratio: float = 0.9,
    utm_zone: Optional[int] = None,
    plot: bool = False,
    csv_path: Optional[str] = None,
) -> Dict[str, object]:
    """
    基于四边形(或任意多边形)经纬度顶点生成“割草式”扫描航线，并返回/保存航点。

    参数:
        lat_list        : 多边形各顶点纬度(度)，顺序为多边形边界顺时针或逆时针。
        lon_list        : 多边形各顶点经度(度)，与 lat_list 对应长度一致。
        fov_deg         : 相机水平视场角(度)。
        altitude_m      : 飞行高度(米)。
        overlap_ratio   : 相邻扫描线步距系数(0~1)。典型 0.8~0.95，数值越小重叠越多。
        utm_zone        : 指定UTM分区(1-60)。若为 None，则根据区域经度自动判断。
        plot            : 是否绘图展示结果。
        csv_path        : 如提供路径，则保存 CSV (列: lat, lon)。

    返回:
        {
          "polygon_utm": shapely Polygon (UTM坐标),
          "scan_lines":  List[LineString 或 MultiLineString] (UTM),
          "waypoints_utm": List[Tuple[float, float]],     # (x, y)
          "waypoints_wgs84": List[Tuple[float, float]],   # (lat, lon)
          "utm_zone": int
        }
    """
    assert len(lat_list) == len(lon_list) and len(lat_list) >= 3, "顶点数量不足或经纬度长度不一致"

    # ---- 1) 自动判断UTM分区（北半球）或使用显式指定 ----
    if utm_zone is None:
        mean_lon = sum(lon_list) / len(lon_list)
        utm_zone = int((mean_lon + 180) // 6) + 1  # 1~60

    # EPSG:326XX 表示北半球；若在南半球，请改为 327XX
    epsg_utm = 32600 + utm_zone

    # ---- 2) 坐标转换：WGS84 -> UTM ----
    to_utm = Transformer.from_crs("EPSG:4326", f"EPSG:{epsg_utm}", always_xy=True)
    from_utm = Transformer.from_crs(f"EPSG:{epsg_utm}", "EPSG:4326", always_xy=True)

    # 注意 always_xy=True 时，输入输出顺序均为 (lon, lat) / (x, y)
    x_list, y_list = to_utm.transform(lon_list, lat_list)

    polygon_xy = list(zip(x_list, y_list))
    poly = Polygon(polygon_xy)
    if not poly.is_valid:
        # 尝试修复自交等问题
        poly = poly.buffer(0)
        if not poly.is_valid:
            raise ValueError("多边形无效，请检查顶点顺序或位置。")

    # ---- 3) 根据相机参数计算扫描步距 ----
    fov_rad = math.radians(fov_deg)
    coverage_width = 2 * altitude_m * math.tan(fov_rad / 2.0)  # 单条扫描线的覆盖宽度
    step = overlap_ratio * coverage_width
    if step <= 0:
        raise ValueError("无效的步距（overlap_ratio * coverage_width <= 0）。")

    # ---- 4) 生成纵向扫描线并裁剪到多边形 ----
    minx, miny, maxx, maxy = poly.bounds
    x = minx
    lines = []
    # 适度外扩，避免边界裁剪遗漏
    padding = max(10.0, 0.1 * (maxy - miny))

    while x <= maxx + 1e-6:
        raw_line = LineString([(x, miny - padding), (x, maxy + padding)])
        clipped = raw_line.intersection(poly)

        # 归一化：把 GeometryCollection 里有效的线段提出来
        segments = []
        if clipped.is_empty:
            pass
        elif isinstance(clipped, (LineString,)):
            segments = [clipped]
        else:
            # MultiLineString 或 GeometryCollection
            for geom in getattr(clipped, "geoms", []):
                if isinstance(geom, LineString) and not geom.is_empty:
                    segments.append(geom)

        if segments:
            # 若同一x多段线，合并相邻段（安全起见做一次unary_union）
            merged = unary_union(segments)
            lines.append(merged)

        x += step

    # ---- 5) 提取路径点（蛇形走位） ----
    waypoints_xy: List[Tuple[float, float]] = []
    for i, seg in enumerate(lines):
        def extend_pts(g):
            pts = list(g.coords)
            if i % 2 == 1:
                pts.reverse()
            return pts

        if isinstance(seg, LineString):
            waypoints_xy.extend(extend_pts(seg))
        else:
            # MultiLineString：将各段串起来（蛇形顺序以 i 的奇偶控制）
            parts = list(seg.geoms)
            # 为提高路径连贯性，可按 y 或 x 排序；这里按 y 均值排序（从下到上）
            parts.sort(key=lambda g: (g.coords[0][1] + g.coords[-1][1]) / 2.0)
            if i % 2 == 1:
                parts.reverse()
            for g in parts:
                waypoints_xy.extend(extend_pts(g))

    if not waypoints_xy:
        raise RuntimeError("生成的航点为空，请检查多边形规模与参数设置。")

    # ---- 6) UTM -> WGS84 航点 ----
    x_wp, y_wp = zip(*waypoints_xy)
    lon_wp, lat_wp = from_utm.transform(x_wp, y_wp)  # 输出顺序: lon, lat
    waypoints_wgs84 = list(zip(lat_wp, lon_wp))

    # ---- 7) 可选保存CSV ----
    if csv_path:
        with open(csv_path, "w", newline="") as f:
            writer = csv.writer(f)
            writer.writerow(["lat", "lon"])
            writer.writerows(waypoints_wgs84)

    # ---- 8) 可选绘图 ----
    if plot:
        fig, ax = plt.subplots(figsize=(8, 8))
        x_poly, y_poly = poly.exterior.xy
        ax.plot(x_poly, y_poly, 'k-', label='Scan Area')

        # 扫描线
        def plot_line_or_multi(gobj):
            if isinstance(gobj, LineString):
                x_line, y_line = gobj.xy
                ax.plot(x_line, y_line, linewidth=1)
            else:
                for gg in gobj.geoms:
                    x_line, y_line = gg.xy
                    ax.plot(x_line, y_line, linewidth=1)

        for seg in lines:
            plot_line_or_multi(seg)

        # 航点
        ax.plot(x_wp, y_wp, '.-', label='Waypoint Path')
        ax.set_aspect('equal')
        ax.set_xlabel("X (m)")
        ax.set_ylabel("Y (m)")
        ax.grid(True)
        ax.legend()
        plt.show()

    return {
        "polygon_utm": poly,
        "scan_lines": lines,
        "waypoints_utm": waypoints_xy,
        "waypoints_wgs84": waypoints_wgs84,
        "utm_zone": utm_zone,
    }


# ===== 示例用法 =====
if __name__ == "__main__":
    lat_list = [30.51935833, 30.5179598, 30.51761963, 30.51823611]
    lon_list = [120.72055278, 120.7189677, 120.72053888, 120.72221312]

    result = plan_coverage_path(
        lat_list, lon_list,
        fov_deg=75, altitude_m=5, overlap_ratio=0.9,#覆盖面积
        utm_zone=None,            # 自动判断Zone
        plot=False,               # 需要看图设为 True
        csv_path="../src/waypoints.csv"  # 保存经纬度航点
    )

    print("UTM分区:", result["utm_zone"])
    print("航点(经纬度)前10个:", result["waypoints_wgs84"][:10])
